import rospy
from webots_ros.srv import set_int

class SensorsInit():
    def __init__(self):
        webots_ns = '/robot'
        kinect_color = '{}/kinect_color/{}'
        kinect_range = '{}/kinect_range/{}'
<<<<<<< HEAD

=======
        
>>>>>>> c0510275eefbacce735e55f10331081ade93d7ac
        rospy.wait_for_service(kinect_color.format(webots_ns,'enable'))
        self.kinect_color_enable = rospy.ServiceProxy(
            kinect_color.format(webots_ns, 'enable'), set_int, persistent=True)

        rospy.wait_for_service(kinect_color.format(webots_ns,'recognition_enable'))
        self.kinect_color_recognition_enable = rospy.ServiceProxy(
            kinect_color.format(webots_ns, 'recognition_enable'), set_int, persistent=True)
<<<<<<< HEAD
        print("----get desirable services, all init done----")

        rospy.wait_for_service("/robot/touch_sensor/enable")
        self.touch_sensor_enable = rospy.ServiceProxy("/robot/touch_sensor/enable", set_int, persistent=True)
=======
        print("----get desirable services----")
>>>>>>> c0510275eefbacce735e55f10331081ade93d7ac

    def enable_sensors(self):
        self.kinect_color_enable(1)
        self.kinect_color_recognition_enable(1)
<<<<<<< HEAD
        self.touch_sensor_enable(1)
=======
>>>>>>> c0510275eefbacce735e55f10331081ade93d7ac

if __name__ == "__main__":

    rospy.init_node('SensorsInitNode', anonymous= False)

    si = SensorsInit()
    si.enable_sensors()
